from point import Point
from rob import Rob
from rrr import RRR
from rrp import RRP

import tools

import math as m
import sympy as sp
from sympy.abc import t


mechanism_list = []
def movement_system_init():
  global bf_rob

  point = {
    'A':Point(0,0),
    'B':None,
    'D':Point(0.13,0)
  }
  l = {
    'AB':0.09,
    'BC':0.15,
    'CD':0.18,
    'EF':0.025,
    'BE':0.1
  }
  l['BF'] = m.sqrt(l['BE']**2+l['EF']**2)
  
  symbol_phi = {
    'AB':100*t
  }

  power_rob = Rob(l['AB'],point['A'],symbol_phi['AB'])
  point['B'] = power_rob.endPoint

  rrr = RRR(point['B'],point['D'],l['BC'],l['CD'],clockwise=True)
  symbol_phi['BC'] = rrr.rob1.symbol_phi
  symbol_phi['BF'] = symbol_phi['BC']+sp.atan2(l['EF'],l['BE'])

  bf_rob = Rob(l['BF'],point['B'],symbol_phi['BF'])

  mechanism_list.append(power_rob)
  mechanism_list.append(rrr)
  mechanism_list.append(bf_rob)


def drawUI():
  import ui
  for mech in mechanism_list:ui.add_mechanism(mech)

  ui.start_draw(0,10000,2)


if __name__ == '__main__':
  movement_system_init()
  # drawUI()

  interval_time = m.pi/18 # pi/180*1000/100,主动件转动一度要花的时间
  args = tools.calc_point_F(bf_rob,interval_time)# 计算点F的位置、运动参数
  tools.toJSON(args)

  args = tools.fromJSON()
  tools.toExcel(args)
  tools.draw_xy(args)
  tools.draw_v(args)
  tools.draw_a(args)
